#ifndef IMMUKF_H
#define IMMUKF_H

#include <iostream>
#include <vector>
#include <queue>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

#include "Ukf.hpp"
#include "Eigen/Geometry"

#include <opencv2/opencv.hpp>

using namespace std;
using namespace Eigen;
using namespace pcl;

struct TrackingResults
{
	vector<int> id;
	vector<double> vel;
	vector<Vector3d> color;
	// bbox 8 pts and traj
	vector<PointCloud<PointXYZ>> bPtsAndTrajPts;
	// box xyz and yaw
	vector<Vector4d> bxyzAndYaw;

	PointCloud<PointXYZ> egoTraj;
};


class ImmUkf
{
private:
	// capacity of vector<UKF*> _targets;
	static const int _targetCapacity = 50;

    double _gammaG;
    double _pD;
    double _pG;
	
	double  _distanceThres; 
	
	double _nearestEuclidThresh;

    double _preTimeStamp;
    vector<UKF*> _targets;
	
	// used for update bbox
	int _updateBBNum;
	double _yUpdateBBTh;
	//double _large_area_th;
	//int _large_box_num;
	
	// total time for prediction
	double _predTime;

	UkfParams _ukf_params;

	double _detP_th;
	double _detS_th;

	double _newTargetDistTh;
	double _inputMeasDistTh;

	UKF _egoUkf;

	queue<pair<double, Vector2d>> ego_xy_history;
	vector<vector<double>> ego_traj;

public:

    // input bbox and center points
    vector<PointCloud<PointXYZ>> _trackBoxes;
	vector<VectorXd> _trackCp;
	
	//int _large_box_cnt;

    ImmUkf();
	
	void read(const cv::FileNode& node)
	{
		node["gammaG"] >> _gammaG;
		node["pD"] >> _pD;
		node["pG"] >> _pG;

		node["nearestEuclidThresh"] >> _nearestEuclidThresh;

		//node["largeAreaThresh"] >> _large_area_th;
		node["yUpdateBBTh"] >> _yUpdateBBTh;
		node["updateBBNum"] >> _updateBBNum;
		
		node["preTime"] >> _predTime;
		
		node["UkfParams"] >> _ukf_params;
		node["detPTh"] >> _detP_th;
		node["detSTh"] >> _detS_th;
		node["newTargetDistTh"] >> _newTargetDistTh;
		node["inputMeasDistTh"] >> _inputMeasDistTh;

		_ukf_params.gammaG = _gammaG;
		_ukf_params.pD = _pD;
		_ukf_params.pG = _pG;

	}

	void setInput(double timestamp, const Vector3d& ego, const vector<PointCloud<PointXYZ>>& input);

	void initEgoTracking();

    void run(double timeStamp);

	void findMaxZandS(int id, VectorXd& maxDetZ, MatrixXd& maxDetS);
	
	void findBestZandS(int id, VectorXd& bestZ, MatrixXd& maxDetS);
	
	void findBestZandMaxS(int id, VectorXd& bestZ, MatrixXd& maxDetS);

	void measurementValidation(int id, VectorXd maxDetZ, MatrixXd maxDetS, vector<VectorXd>& measVec, vector<bool>& measTaken, vector<int>& measIds);

	void getNearestEuclidBBox(const VectorXd& cp, const vector<VectorXd>& measVec, int& minInd);

	void associateBB(int id, VectorXd maxDetZ, vector<VectorXd>& measVec, vector<int>& measIds);

	void updateBB(int id, const vector<int>& measIds);
	
	void filterPDA(int id, vector<VectorXd> measVec, vector<double>& lambdaVec);

	void updateTrackingState(int id, int numOfMeas, const vector<VectorXd>& measVec);

	void mergeOverSegmentation();

	void updateTargets(const vector<bool>& measTaken, double timeStamp);

	void removeDuplicatedTarget(int id);

	TrackingResults getArrangeResults();	

	~ImmUkf();
};

static void read(const cv::FileNode& node, ImmUkf& x, const ImmUkf& default_value = ImmUkf()){
    if(node.empty())
        x = default_value;
    else
        x.read(node);
}

#endif /* IMMUKF_H */
